#include "oslib.h"
#include "vesc_can.h"
#include "shooter.h"

//本杰明状态包信息
struct VESC_status
{
    int id;
    //status1
    int32_t rpm;                         //转速
    int16_t tot_current_filtered;        //总电流滤波
    int16_t duty_cycle_now;              //占空比
    //status2
    int32_t amp_hours;                   //从输入源提取的安培小时数
    int32_t amp_hours_charged;           //反馈到输入源的安培小时数
    //status3
    int32_t watt_hours;                  //从输入源提取的瓦时数
    int32_t watt_hours_charged;          //反馈到输入源的瓦时数
    //status4
    int16_t temp_fet_filtered;           //滤波后的MOSFET温度。温度是预先计算的，所以这个函数很快
    int16_t temp_motor_filtered;         //滤波后的电机温度。温度是预先计算的，所以这个函数很快
    int16_t tot_current_in_filtered;     //
    int16_t pid_pos_now;                 //位置
    //status5
    int32_t tachometer_value;            //转速表值
    int16_t INPUT_VOLTAGE;               //输入电压
    int16_t tmp;                         //0
};

void shooter_vesc_task(void *argument)
{
    // Legacy
    osThreadExit();
    extern osMessageQueueId_t ShooterVescQueueHandle;
    for (;;) {
        static CAN_ConnMessage msg;
        static struct VESC_status vesc_status;
        osMessageQueueGet(ShooterVescQueueHandle, &msg, NULL, osWaitForever);
        /* 处理 */
        int vesc_pack_id = (msg.id >> 8) & 0x1F;
        int vesc_id = msg.id & 0xFF;
        int32_t index = 0;
        switch (vesc_pack_id) {
        case CAN_PACKET_STATUS_4:
            vesc_status.temp_fet_filtered = buffer_get_int16(msg.payload.ui8, &index) / 1e1;
            vesc_status.temp_motor_filtered = buffer_get_int16(msg.payload.ui8, &index) / 1e1;
            vesc_status.tot_current_in_filtered = buffer_get_int16(msg.payload.ui8, &index) / 1e1;
            vesc_status.pid_pos_now = buffer_get_int16(msg.payload.ui8, &index) / 50;
            shooter_force_update(shooter1, vesc_status.pid_pos_now);
            break;
        default:
            break;
        }
    }
}

